/**
 * @file
 * @brief Implementation of CanbusManager
 */

#include <ros/ros.h>
#include "canbus/manager.h"
#include <iostream>

using namespace canbus;

CanbusManager::CanbusManager():
channel_1("main_channel", 0, canBITRATE_250K, canMSG_STD),
channel_2("encoder_channel", 0, canBITRATE_250K, canMSG_EXT)
{}

void CanbusManager::Init() {
  channel_1.Init();
  channel_2.Init();
  channel_1.Configure();
  channel_2.Configure();  

  // add channel 1 msgs
  channel_1.RegisterCanMsg<CanMsg10>();
  channel_1.RegisterCanMsg<CanMsg11>();
  channel_1.RegisterCanMsg<CanMsg12>();
  channel_1.RegisterCanMsg<CanMsg13>();
  channel_1.RegisterCanMsg<CanMsg14>();
  channel_1.RegisterCanMsg<CanMsg15>();
  channel_1.RegisterCanMsg<CanMsg16>();
  channel_1.RegisterCanMsg<CanMsg17>();
  channel_1.RegisterCanMsg<CanMsg20>();
  channel_1.RegisterCanMsg<CanMsg21>();
  channel_1.RegisterCanMsg<CanMsg22>();

  // add channel 2 msgs
  channel_2.RegisterCanMsg<CanMsgEncoderFRReq>();
  channel_2.RegisterCanMsg<CanMsgEncoderFLReq>();
  channel_2.RegisterCanMsg<CanMsgEncoderRRReq>();
  channel_2.RegisterCanMsg<CanMsgEncoderRLReq>();
  channel_2.RegisterCanMsg<CanMsgEncoderFR>();
  channel_2.RegisterCanMsg<CanMsgEncoderFL>();
  channel_2.RegisterCanMsg<CanMsgEncoderRR>();
  channel_2.RegisterCanMsg<CanMsgEncoderRL>();
}

VehicleStatus* CanbusManager::ReadVehicleStatus() {
  vehicle_status_.update(dynamic_cast<CanMsg10*>(channel_1.GetDataById(10)));
  return &vehicle_status_;
}

VehicleState* CanbusManager::ReadVehicleState() {
  vehicle_state_.update(dynamic_cast<CanMsg11*>(channel_1.GetDataById(11)));
  return &vehicle_state_;
}

SpreaderState* CanbusManager::ReadSpreaderState() {
  spreader_state_.update(dynamic_cast<CanMsg12*>(channel_1.GetDataById(12)), 
                         dynamic_cast<CanMsg10*>(channel_1.GetDataById(10)));
  return &spreader_state_;
}

VehicleSensor* CanbusManager::ReadVehicleSensor() {
  vehicle_sensor_.update(dynamic_cast<CanMsg10*>(channel_1.GetDataById(10)),
                         dynamic_cast<CanMsg13*>(channel_1.GetDataById(13)),
                         dynamic_cast<CanMsg14*>(channel_1.GetDataById(14)),
                         dynamic_cast<CanMsg15*>(channel_1.GetDataById(15)));
  return &vehicle_sensor_;
}

WheelEncoder* CanbusManager::ReadWheelEncoder() {
  wheel_encoder_.update(dynamic_cast<CanMsg10*>(channel_1.GetDataById(10)),
                        dynamic_cast<CanMsg16*>(channel_1.GetDataById(16)),
                        dynamic_cast<CanMsg17*>(channel_1.GetDataById(17)));
  return &wheel_encoder_;
}

void CanbusManager::WriteVehicleCmd(const VehicleCmd* vcmd) {
  CanMsg20* ptr = dynamic_cast<CanMsg20*>(channel_1.GetDataById(20));
  ptr->DownUpdate(*vcmd);
  channel_1.Write(ptr);
}

void CanbusManager::WriteVehicleCtrl(const VehicleCtrl* vctrl) {
  CanMsg21* ptr = dynamic_cast<CanMsg21*>(channel_1.GetDataById(21));
  ptr->DownUpdate(*vctrl);
  channel_1.Write(ptr);
}

void CanbusManager::WriteSpreaderCmd(const SpreaderCtrl* sctrl) {
  CanMsg22* ptr = dynamic_cast<CanMsg22*>(channel_1.GetDataById(22));
  ptr->DownUpdate(*sctrl);
  channel_1.Write(ptr);
}

void CanbusManager::Start() {
  channel_1.Start();
  channel_2.Start();
}

void CanbusManager::Stop() {
  channel_1.Stop();
  channel_2.Stop();
}